#include <p32xxxx.h>

#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"
#include "semphr.h"

#include "driverI2C_ISR.h"
#include "main_peripherals.h"
#include "rtos_queues.h"

struct i2cAction_t * currentAction;
unsigned char i2cState;
unsigned char i2cLength;

void i2cQueueAction(struct i2cAction_t * action)
{
    extern SemaphoreHandle_t semI2C;
    if (xSemaphoreTake(semI2C,0) == pdTRUE)
    {
        currentAction = action;
        i2cState = I2CS_START;
        currentAction->state = I2C_STATE_BUSY;
        i2cLength = currentAction->writeLength;

        I2C2CONSET = _I2C2CON_SEN_MASK;

        if (I2C2STATbits.IWCOL)
//        if (I2CStart(I2C2) != I2C_SUCCESS)
        {
            while(1)
            {
                portNOP();
            }
        }
    }
    else
    {
        extern QueueHandle_t queI2C;
        if (xQueueSendToBack(queI2C,&action,0) != pdPASS)
        {
            action->state = I2C_STATE_QUEUEERROR;
            if (action->callbackFunction) action->callbackFunction(NULL);
        }
    }
}

void i2cQueueActionFromISR(struct i2cAction_t * action)
{
    extern SemaphoreHandle_t semI2C;
    if (xSemaphoreTakeFromISR(semI2C,NULL) == pdTRUE)
    {
        currentAction = action;
        currentAction->state = I2C_STATE_BUSY;
        i2cLength = currentAction->writeLength;
        i2cState = I2CS_START;

        I2C2CONSET = _I2C2CON_SEN_MASK;

        if (I2C2STATbits.IWCOL)
        //if (I2CStart(I2C2) != I2C_SUCCESS)
        {
            while(1)
            {
                portNOP();
            }
        }
    }
    else
    {
        extern QueueHandle_t queI2C;
        if (xQueueSendToBackFromISR(queI2C,&action,NULL) != pdPASS)
        {
            action->state = I2C_STATE_QUEUEERROR;
            if (action->callbackFunction) action->callbackFunction(NULL);
        }
    }
}

void i2cChannelReset(BYTE I2CChannel)
{
    if (I2CChannel == 0) I2C1CONCLR = _I2C1CON_ON_MASK;
    else I2C2CONCLR = _I2C2CON_ON_MASK;

    //I2CEnable(I2CChannel,FALSE);
    int i;
    I2C_DAT_SET_OUT(I2CChannel);
    I2C_DAT_SET(I2CChannel);
    I2C_CLK_SET_OUT(I2CChannel);
    I2C_CLK_SET(I2CChannel);
    //I2C_DAT_CLR(I2CChannel);
    for (i=0;i<1000;i++) portNOP();
    I2C_DAT_CLR(I2CChannel);
    for (i=0;i<1000;i++) portNOP();
    I2C_DAT_SET(I2CChannel);

    int j;
    for (j=0;j<9;j++)
    {
        I2C_CLK_CLR(I2CChannel);
        for (i=0;i<2000;i++) portNOP();
        I2C_CLK_SET(I2CChannel);
        for (i=0;i<2000;i++) portNOP();
    }
    I2C_DAT_CLR(I2CChannel);
    for (i=0;i<2000;i++) portNOP();

    I2C_DAT_SET(I2CChannel);
    for (i=0;i<1000;i++) portNOP();
    //I2C_DAT_SET(I2CChannel);

    if (I2CChannel == 0) I2C1CONSET = _I2C1CON_ON_MASK;
    else I2C2CONSET = _I2C2CON_ON_MASK;
    //I2CEnable(I2CChannel,TRUE);
}

void i2cInit()
{
    extern SemaphoreHandle_t semI2C;
    //while (1)
    {
        i2cChannelReset(0);
        i2cChannelReset(1);
        xSemaphoreGive(semI2C);
    }
}

void __attribute__((vector(_I2C2_VECTOR), interrupt(ipl6))) ISRWRAPPER_I2C2();

void ISR_I2C2()
{
    static portBASE_TYPE higherPriorTask;
    higherPriorTask = NULL;

    /*if (INTGetFlag(INT_SOURCE_I2C_ERROR(I2C2)))
    {
        I2CEnable(I2C2,0);
        INTClearFlag(INT_SOURCE_I2C_ERROR(I2C2));
        INTClearFlag(INT_SOURCE_I2C_MASTER(I2C2));
        i2cChannelReset(I2C2);
        I2CEnable(I2C2,1);
    }*/
    if (IFS1bits.I2C2BIF)
    {
        I2C2CONCLR = _I2C2CON_ON_MASK;
        IFS1CLR = _IFS1_I2C2MIF_MASK | _IFS1_I2C2BIF_MASK;
        i2cChannelReset(1);
        I2C2CONSET = _I2C2CON_ON_MASK;

        i2cState = I2CS_DONE;
        if (currentAction != NULL)
            currentAction->state = I2C_STATE_ERROR;

        extern SemaphoreHandle_t semI2C;
        xSemaphoreGiveFromISR(semI2C, &higherPriorTask);
    }


    if (IFS1bits.I2C2MIF)
//    if (INTGetFlag(INT_SOURCE_I2C_MASTER(I2C2)))
    {
        //INTClearFlag(INT_SOURCE_I2C_MASTER(I2C2));
        IFS1CLR = _IFS1_I2C2MIF_MASK;
        
        if (i2cState & 0x80 == 0x80)
        {
            if (I2C2STATbits.ACKSTAT)
            //if (!I2CByteWasAcknowledged(I2C2))
            {
                currentAction->state = I2C_STATE_ERROR;
                i2cState = I2CS_STOPPED;
                //I2CStop(I2C2);
                I2C2CONSET = _I2C2CON_PEN_MASK;
                return;
            }
        }

        if (i2cState == I2CS_START)
        {
            I2C2TRN = currentAction->address;
            //I2CSendByte(I2C2, currentAction->address);
            i2cState = I2CS_WRITE_ADDRESS_ACK;
        }
        else if (i2cState == I2CS_RESTART)
        {
            //I2CSendByte(I2C2, currentAction->address | 0x01);
            I2C2TRN = currentAction->address | 0x01;
            i2cState = I2CS_READ_ADDRESS_ACK;
        }
        else if (i2cState == I2CS_WRITE_ADDRESS_ACK)
        {
            if (currentAction->writeLength > 0)
            {
                i2cLength = currentAction->writeLength;
                i2cState = I2CS_WRITE;
//                I2CSendByte(I2C2, currentAction->writePacket[0]);
                I2C2TRN = currentAction->writePacket[0];
                i2cLength--;
            }
            else if (currentAction->readLength > 0)
            {
//                I2CRepeatStart(I2C2);
                I2C2CONSET = _I2C2CON_RSEN_MASK;
                i2cState = I2CS_RESTART;
            }
            else
            {
//                I2CStop(I2C2);
                I2C2CONSET = _I2C2CON_PEN_MASK;
                i2cState = I2CS_STOPPED;
            }
        }
        else if (i2cState == I2CS_READ_ADDRESS_ACK)
        {
            i2cLength = currentAction->readLength;
            i2cState = I2CS_READ;

            //I2CReceiverEnable(I2C2, TRUE);
            I2C2CONSET = _I2C2CON_RCEN_MASK;
        }
        else if (i2cState == I2CS_WRITE)
        {
            if (i2cLength > 0)
            {
//                I2CSendByte(I2C2,currentAction->writePacket[currentAction->writeLength - i2cLength]);
                I2C2TRN = currentAction->writePacket[currentAction->writeLength - i2cLength];
                i2cLength--;
            }
            else
            {
                if (currentAction->readLength > 0)
                {
                    i2cLength = currentAction->readLength;
//                    I2CRepeatStart(I2C2);
                    I2C2CONSET = _I2C2CON_RSEN_MASK;
                    i2cState = I2CS_RESTART;
                }
                else
                {
//                    I2CStop(I2C2);
                    I2C2CONSET = _I2C2CON_PEN_MASK;
                    i2cState = I2CS_STOPPED;
                }
            }
        }
        else if (i2cState == I2CS_READ)
        {
//            currentAction->readPacket[currentAction->readLength - i2cLength] = I2CGetByte(I2C2);
            currentAction->readPacket[currentAction->readLength - i2cLength] = I2C2RCV;
            i2cLength--;
            i2cState = I2CS_READ_ACK;
            if (i2cLength > 0)
            {
//                I2CAcknowledgeByte(I2C2, TRUE);
                I2C2CONCLR = _I2C2CON_ACKDT_MASK;
                I2C2CONSET = _I2C2CON_ACKEN_MASK;
            }
            else
            {
//                I2CAcknowledgeByte(I2C2, FALSE);
                I2C2CONSET = _I2C2CON_ACKDT_MASK | _I2C2CON_ACKEN_MASK;
            }
        }
        else if (i2cState == I2CS_READ_ACK)
        {
            if (i2cLength > 0)
            {
                i2cState = I2CS_READ;
//                I2CReceiverEnable(I2C2, TRUE);
                I2C2CONSET = _I2C2CON_RCEN_MASK;
            }
            else
            {
                i2cState = I2CS_STOPPED;
//                I2CStop(I2C2);
                I2C2CONSET = _I2C2CON_PEN_MASK;
            }
        }
        else if (i2cState == I2CS_STOPPED)
        {
            if (currentAction->state == I2C_STATE_BUSY) currentAction->state = I2C_STATE_DONE;

            if (currentAction->callbackFunction != NULL) currentAction->callbackFunction(&higherPriorTask);

            extern QueueHandle_t queI2C;
            if (xQueueReceiveFromISR(queI2C, &currentAction, NULL) == pdTRUE)
            {
                currentAction->state = I2C_STATE_BUSY;
                i2cState = I2CS_START;
//                I2CStart(I2C2);
                I2C2CONSET = _I2C2CON_SEN_MASK;
            }
            else
            {
                i2cState = I2CS_DONE;
                currentAction = NULL;
                extern SemaphoreHandle_t semI2C;
                xSemaphoreGiveFromISR(semI2C, &higherPriorTask);
            }
        }
    }
    portEND_SWITCHING_ISR(higherPriorTask);
}


